﻿using Emgu.CV;
using Emgu.CV.GPU;
using Emgu.CV.Structure;
using FinalYear.Helpers;
using System.Drawing;

namespace FinalYear.iNavigate.Classes
{
    public class OpticalFlowGPU
    {
        private GpuMat<float> PreviousFoundFeatures;

        private GpuMat<float> TrackedFeatures;

        public GpuMat<byte> status;

        public GpuMat<float> error;

        private GpuPyrLKOpticalFlow flow;
        private GpuImage<Gray, byte> PreviousGrayImage;
        private GpuImage<Gray, byte> CurrentGrayImage;

        public OpticalFlowGPU()
        {
            if (GpuInvoke.HasCuda)
            {
                this.flow = new GpuPyrLKOpticalFlow(new Size(Constants.WINDOWSIZE, Constants.WINDOWSIZE), 5, 20, 0.5, false, 1e-4f);
            }
        }

        public GpuMat<float> GoodFeaturesToTrack(Image<Gray, byte> previousGrayImage, Image<Gray, byte> currentGrayImage, int maxFeatureCount, double qualityLevel, double minDistance)
        {
            if (GpuInvoke.HasCuda)
            {
                this.PreviousGrayImage = new GpuImage<Gray, byte>(previousGrayImage);
                this.CurrentGrayImage = new GpuImage<Gray, byte>(currentGrayImage);

                GpuGoodFeaturesToTrackDetector det = new GpuGoodFeaturesToTrackDetector(maxFeatureCount, qualityLevel, minDistance);
                return det.Detect(this.CurrentGrayImage, new GpuImage<Gray, byte>());
            }

            return null;
        }

        public GpuMat<float> ComputeFlow()
        {
            if (this.PreviousGrayImage != null)
            {
                this.flow.Sparse(this.PreviousGrayImage, this.CurrentGrayImage, this.PreviousFoundFeatures, out this.TrackedFeatures, out this.status, out error);
            }

            return this.TrackedFeatures;
        }
    }
}